//
//  LBPathFind.m
//  LittleBall
//
//

#import "LBPathFind.h"

@implementation LBPathFind

@synthesize pathLength;


- (id) init
{
    self = [super init];
    if (self) 
    {
        // Initialization code here.
    }
    
    pathStorage = nil;
    randLoc = nil;
    
    return self;
}


- (void) initializePathFind 

{
    pathStorage = (int*)malloc(8);
}


- (void) terminatePathFind
{
    free (pathStorage);
}



//-----------------------------------------------------------------------------------
// 描述: 查找最短路径
//      最短路径的判定基于F值，F=G+H，G是从起点到当前区块的路径长度，H是当前区块到终点的曼哈顿启发式距离 
//      H=(yTarget-yCurrent)+(xTarget-xCurrent）
//      每一次路径步进，选择相邻的F值最小的区块，最终使得总路径最短
//
// startingX/Y: 经过处理的区块中心坐标索引，例如10*10地图最左上方区块中心为（0，0），最右下方区块中心为（10，10）
//              需要将实际的touchpoint转换成该索引（任意地图都可以划分为大小可变的区块，具有普适性）
//-----------------------------------------------------------------------------------

- (int) FindPath:(int)startingX yStart:(int)startingY xTarget:(int)targetX yTarget:(int)targetY
{
    int onOpenList=0, onClosedList=10, parentXval=0, parentYval=0,
	a=0, b=0, m=0, u=0, v=0, temp=0, numberOfOpenListItems=0,
	addedGCost=0, tempGcost=0, path = 0,
	tempx, pathX, pathY, cellPosition,             
	newOpenListItemID=0;
    
	int startX = startingX;
	int startY = startingY;	
    
    //	如果起始区块和终点区块是同一个区块
	if (startX == targetX && startY == targetY && pathLocation > 0)
		return found;
	if (startX == targetX && startY == targetY && pathLocation == 0)
		return nonexistent;
    
    //	如果终点区块属于障碍物区块，则返回路径不存在
	if (walkability[targetX][targetY] == unwalkable)
		goto noPath;
    
    //  初始化状态
    for (int x = 0; x < mapWidth; x++){
        for (int y = 0; y < mapHeight; y++) {
            whichList[x][y] = 5;
        }
    }
    
	pathLength = notStarted; // 0
	pathLocation = notStarted; // 0
	Gcost[startX][startY] = 0; // 设置起点区块的G值为0
    
    //  将起点区块放入开集
	numberOfOpenListItems = 1;
	openList[1] = 1;
	openX[1] = startX ; openY[1] = startY;
    
	do
    {
        //  如果开集非空，则弹出开集第一个元素至闭集
        //  代表被选中为路径上的路由区块（节点）
        if (numberOfOpenListItems != 0)
        {
            
            parentXval = openX[openList[1]];
            parentYval = openY[openList[1]]; // 记录即将加入闭集的区块的位置信息
            whichList[parentXval][parentYval] = onClosedList; // 标识加入闭集的区块的状态
            
            numberOfOpenListItems = numberOfOpenListItems - 1;
            
            //	从堆中删除首元素后，需要将最末尾的元素提升至首位，并与子节点元素比较F值，重新整理堆数组元素的顺序
            openList[1] = openList[numberOfOpenListItems+1];
            v = 1;
            
            do
            {
                u = v;		
                if (2*u+1 <= numberOfOpenListItems)
                {
                    
                    if (Fcost[openList[u]] >= Fcost[openList[2*u]]) 
                        v = 2*u;
                    if (Fcost[openList[v]] >= Fcost[openList[2*u+1]]) 
                        v = 2*u+1;		
                }
                else
                {
                    if (2*u <= numberOfOpenListItems) 
                    {
                        
                        if (Fcost[openList[u]] >= Fcost[openList[2*u]]) 
                            v = 2*u;
                    }
                }
                
                if (u != v) 
                {
                    temp = openList[u];
                    openList[u] = openList[v];
                    openList[v] = temp;			
                }
                else
                    break; 
                
            }
            while (1);
            
            //  检查相邻区            
            for (b = parentYval-1; b <= parentYval+1; b++)
            {
                for (a = parentXval-1; a <= parentXval+1; a++)
                {
                    //  相应区块特定情形的判定: 是否超出边界；是否处于闭集；是否可通过 (a,b为相邻区块坐标）
                    if (a == parentXval || b == parentYval) 
                    {
                        
                        if (a != -1 && b != -1 && a != mapWidth && b != mapHeight)
                        {

                            if (whichList[a][b] != onClosedList)
                            {
                                if (walkability[a][b] == walkable)
                                { 
                                    if (1) 
                                    {
                                        // 如果不在开集中，则将该区块加入开集，并调整开集中已有区块的顺序（数组堆第一个元素为F值最小的区块）
                                        if (whichList[a][b] != onOpenList) 
                                        {	
                                            newOpenListItemID = newOpenListItemID + 1;
                                            m = numberOfOpenListItems+1;
                                            openList[m] = newOpenListItemID; // 此时新加入的区块处于开集数组末端
                                            openX[newOpenListItemID] = a;
                                            openY[newOpenListItemID] = b;
                                            
                                            addedGCost = stepsize;	// 步长，可修改成任意正整数
                                            
                                            Gcost[a][b] = Gcost[parentXval][parentYval] + addedGCost;  
                                            
                                            Hcost[openList[m]] = stepsize*(abs(a - targetX) + abs(b - targetY));
                                            Fcost[openList[m]] = Gcost[a][b] + Hcost[openList[m]];
                                            parentX[a][b] = parentXval ; parentY[a][b] = parentYval;	
                                            
                                            while (m != 1) // 整理开集，根据F值调整其中区块顺序
                                            {
                                                if (Fcost[openList[m]] <= Fcost[openList[m/2]])
                                                {
                                                    temp = openList[m/2];
                                                    openList[m/2] = openList[m];
                                                    openList[m] = temp;
                                                    m = m/2;
                                                }
                                                else
                                                    break;
                                            }
                                            numberOfOpenListItems = numberOfOpenListItems+1;
                                            
                                            // 已将区块加入开集，因此改变该区块状态标识为开集
                                            whichList[a][b] = onOpenList;
                                        }
                                        
                                        //  如果区块已存在于开集中，检查到达这个区块的当前路径是否更佳，若G值更小则替换原路径
                                        // （区块已存在于开集中说明之前已经有路径到达该区块，该区块已有原始的G值，比较当前F值和原始G值）
                                        else
                                        {                  
                                            addedGCost = 10;	
                                            
                                            tempGcost = Gcost[parentXval][parentYval] + addedGCost;
                                            
                                            if (tempGcost < Gcost[a][b]) // 满足条件则改变该区块的父区块
                                            {
                                                parentX[a][b] = parentXval; 
                                                parentY[a][b] = parentYval;
                                                Gcost[a][b] = tempGcost;		                           
                                                
                                                for (int x = 1; x <= numberOfOpenListItems; x++) // 遍历查找数组堆，判断并调整F值改变后的该区块在开集中的新位置
                                                {
                                                    if (openX[openList[x]] == a && openY[openList[x]] == b) 
                                                    {
                                                        Fcost[openList[x]] = Gcost[a][b] + Hcost[openList[x]]; // G值变化导致F值相应变化
                                                        
                                                        m = x;
                                                        while (m != 1) 
                                                        {
                                                            if (Fcost[openList[m]] < Fcost[openList[m/2]])
                                                            {
                                                                temp = openList[m/2];
                                                                openList[m/2] = openList[m];
                                                                openList[m] = temp;
                                                                m = m/2;
                                                            }
                                                            else
                                                                break;
                                                        } 
                                                        break; 
                                                    } 
                                                } 
                                            }
                                            
                                        }
                                    }
                                }
                            }
                        }
                    }
                }
            }
            
        }
        
        //  如果开集为空，则表明路径不存在	
        else
        {
            path = nonexistent; break; // 2
        }  
        
        //  如果目标区块被加入开集，则表明寻径成功
        if (whichList[targetX][targetY] == onOpenList)
        {
            path = found; break; // 1
        }
        
    }
	while (1); // 一直循环直到找到路径或证明路径不存在
    
    
	if (path == found)
	{
        
        //  从终点开始沿路径反向搜索父区块直到起点
        pathX = targetX; pathY = targetY;      
        do
        {
            tempx = parentX[pathX][pathY];		
            pathY = parentY[pathX][pathY];
            pathX = tempx;
            
            pathLength = pathLength + 1;
        }
        while (pathX != startX || pathY != startY);   // 计算路径长度          
        
        if( pathStorage != nil )
        {
            free(pathStorage);
            pathStorage = nil;
        }
        
        pathStorage = (int*) malloc (pathLength*2*sizeof(int));  
        
        //  反向存储路径区块信息
        pathX = targetX ; pathY = targetY;
        cellPosition = pathLength*2;	
        do
        {
            cellPosition = cellPosition - 2;
            pathStorage[cellPosition] = pathX;
            pathStorage[cellPosition+1] = pathY;
            
            tempx = parentX[pathX][pathY];		
            pathY = parentY[pathX][pathY];
            pathX = tempx;
        }
        while (pathX != startX || pathY != startY);	
        
        
        [self ReadPath:startingX currentY:startingY speed:1];
        
	}
	return path;
    
    //  如果路径不存在，设置xPath, yPath为起始点，并返回路径不存在
noPath:
	xPath = startingX;
	yPath = startingY;
	return nonexistent;
}

//-----------------------------------------------------------------------------------
// 描述: 从pathStorage中读取位置信息
//      小球在行进中判断是否到了一个新的区块中心，并读取该区块中心位置信息（允许一定的误差 speed）
//
// currentX/Y: 小球的当前位置
// speed:      小球移动的速度
//-----------------------------------------------------------------------------------
- (void) ReadPath:(int)currentX currentY:(int)currentY speed:(int)pixelsPerFrame
{
    
	// 成功找到了路径
	if (pathStatus == found)
	{
        
		if (pathLocation < pathLength)
		{
			if (pathLocation == 0 || 
				(abs(currentX - xPath) < pixelsPerFrame && abs(currentY - yPath) < pixelsPerFrame))
                pathLocation = pathLocation + 1;  // 在一定误差允许范围内判断到新的位置了
		}
        		
		if (pathLocation <= pathLength)
        {
            xPath = pathStorage[pathLocation*2-2];
            xPath = pathStorage[pathLocation*2-1];	
        }
        
        // 已经在终点了
		if (pathLocation == pathLength) 
		{
			if (abs(currentX - xPath) < pixelsPerFrame 
				&& abs(currentY - yPath) < pixelsPerFrame)
                pathStatus = notStarted; 
		}
	}
    
	else
	{	
		xPath = currentX;
		yPath = currentY;
	}
}



////-----------------------------------------------------------------------------------
//// 描述: 设置地图的障碍特性
////-----------------------------------------------------------------------------------
- (void) setWalkabilityCoordX:(int)x CoordY:(int)y walkBility:(MapNodeWalkability)walkBility
{
    walkability[x][y] = walkBility;
}


-(int *) getPathStorage
{
    return pathStorage;
}



//---------------------------------------------------------------------------------------
// 描述: 输出任意个数(number)随机坐标点。返回一维数组randLoc，长度为number*2，记录number个随机节点的x-y坐标值。
//      randLoc[0]是第一个随机节点的x值，randLoc[1]是第一个随机节点的y值，以此类推。
//      该方法可适用放入地图的任意尺寸对象，输出的节点为该对象所占区域的最左上角点。
// number: 随机节点的个数
// width/height: 放入对象的尺寸
//---------------------------------------------------------------------------------------
- (int *) generateRandomObjectsNumber:(int)number Width:(int)width Height:(int)height
{
    int n = 0, walkableCount = 0, randLocIndex = 0;
    int restOfNodes = number;
    int* tempStorageOfWalkableNodes;
    BOOL flag;
    
    tempStorageOfWalkableNodes = malloc(2*mapWidth*mapHeight*sizeof(int));
    
    if( randLoc )
    {
        free(randLoc);
        randLoc = nil;
    }
    
    randLoc = malloc(2*number*sizeof(int)); 
    
    do 
    {
        for (int i = 0; i < mapWidth; i++) 
        {
            for (int j = 0; j < mapHeight; j++) 
            {
                if (walkability[i][j] == walkable) 
                {
                    flag = [self justifyDistrictWidth:width Height:height startPositionX:i startPostionY:j];
                    
                    if (flag)
                    {
                        walkableCount++;
                        tempStorageOfWalkableNodes[n]=i;
                        tempStorageOfWalkableNodes[n+1]=j;
                        n = n + 2;                            
                    }              
                }
            }
        }
        
        if (walkableCount==0 || walkableCount < restOfNodes)
        {
            free(randLoc);
            randLoc = nil;
            break;
        }
        
        int position = arc4random() % walkableCount;
        
        randLoc[randLocIndex] = tempStorageOfWalkableNodes[position*2];
        randLoc[randLocIndex+1] = tempStorageOfWalkableNodes[position*2+1];
        
        for (int x = randLoc[randLocIndex]; x <= (randLoc[randLocIndex]+height-1); x++)
        {
            for (int y = randLoc[randLocIndex+1]; y <= (randLoc[randLocIndex+1]+width-1); y++)
            {
                walkability[x][y] = unwalkable;
            }
        }   
        
        restOfNodes = restOfNodes - 1;
        randLocIndex = randLocIndex + 2;
        
        walkableCount = 0;
        n = 0;
        
    }while(randLocIndex < number*2);
    
    if (randLoc != nil)
    {
        for (int loop = 0; loop <= number*2-2; loop = loop+2)
        {
            walkability[randLoc[loop]][randLoc[loop+1]] = 0;
        } 
    }
    
    free(tempStorageOfWalkableNodes);
    tempStorageOfWalkableNodes = nil;
    
    return randLoc;                    // 调用时要判断landLoc是否为空，为空则表明无法产生满足条件的随机点
}



//---------------------------------------------------------------------------------------
// 描述: 判断对象物体能否放置在地图某个区域上
// width/hegith: 对象物体的尺寸
// startPostionX/Y: 矩形区域最左上角区块节点坐标
//---------------------------------------------------------------------------------------
- (BOOL)justifyDistrictWidth:(int)width Height:(int)height startPositionX:(int)x startPostionY:(int)y
{
    BOOL flag = YES;
    
    if (x + height - 1 >= mapHeight || y + width - 1 >= mapWidth)
    {
        flag = NO;
    }
    
    if (flag)
    {
        for (int i = x; i <= x+width-1; i++)
        {
            if (walkability[i][y] == unwalkable)
                flag = NO;
            break;  
        }
    }    
    
    if (flag) 
    {
        for (int j = y; j <= x+height-1; j++)
        {
            if (walkability[x][j] == unwalkable)
            flag = NO;
            break;
        }
    }
    
    if (flag)
        return YES;
    else
        return NO;
    
}


-(void) dealloc
{
    if( pathStorage != nil )
    {
        free(pathStorage);
        pathStorage = nil;
    }
    
    if( randLoc != nil )
    {
        free(randLoc);
        randLoc = nil;
    }
}

@end
